//
// Created by lyj on 4/20/2023.
//
#include <vector>
// #include "bmutility.h"

#pragma pack(push)
#pragma pack(1)

struct YoloBox {
  int x, y, width, height;
  float score;
  int class_id;
};

using YoloBoxVec = std::vector<YoloBox>;

// DL range
typedef struct
{
    double x;
    double y;
    double z;
} Position3D;

// IMU+UWB test
typedef struct
{
    uint8_t byteArray[3];
} nint24_t;

typedef struct
{
    uint8_t role;
    uint8_t id;
    nint24_t dis;
    uint8_t fp_rssi;
    uint8_t rx_rssi;
    uint32_t system_time;
    uint8_t reserved[2];
} nlt_nodeframe2_node_raw_t;

typedef struct
{
    uint8_t header[2];
    uint16_t frame_length;
    uint8_t role;
    uint8_t id;
    uint32_t system_time;
    uint8_t eop_3d[3];
    nint24_t pos_3d[3];
    nint24_t vel_3d[3];
    uint8_t reserved1[9];
    float imu_gyro_3d[3];
    float imu_acc_3d[3];
    uint8_t reserved2[12];
    int16_t angle_3d[3];
    float quaternion[4];
    uint8_t reserved3[4];
    uint32_t local_time;
    uint8_t reserved4[10];
    uint16_t voltage;
    uint8_t valid_node_count;
    nlt_nodeframe2_node_raw_t nodeframe2_node_raw_t[4];
    uint8_t checkSum;
} nlt_nodeframe2_raw_t;
// end of IMU + UWB test



// com data package function
typedef struct {
	unsigned short Head;   
	unsigned char DataLength;
	unsigned char DataType;
    int rollAngleVelocity;
    int pitchAngleVelocity;
    int yawAngleVelocity;
    int Roll;
    int Pitch;
    int Yaw;
    int Xpos;
    int Ypos;
    int Zpos;
    int axAcceleration;  
    int ayAcceleration;  
    int azAcceleration;  
    int xVelocity;
    int yVelocity;
    int zVelocity;    
    unsigned int  NavPackeCount;
    unsigned char NavFlag;	    
	unsigned char Checksum;    
} TestImuDataStruct;  

typedef struct {
	unsigned short Head;   
	unsigned char DataLength;
	unsigned char ImuState;
    unsigned int timeStamp;
    double Longitude;
    double Latitude;
    float Altitude; 
    float Pitch;
    float Yaw;
    float Roll;
    float EaseVelocity;
    float NorthVelocity;
    float UpVelocity;  
    float ZAngleVelocity;
    float YAngleVelocity;
    float XAngleVelocity;
    float axAcceleration;  
    float ayAcceleration;  
    float azAcceleration;  
    unsigned int  GPSBDsecond;
    unsigned char NavFlag;
    unsigned short ErrorFlag;	    
	unsigned char Checksum;    
} TestImuDataStruct_KDY_5G;  
// test type 0B

typedef struct {
	unsigned char Head1;
	unsigned char Head2;    
	unsigned char DataLength;
	unsigned char DataType;
	unsigned char NodeID;
    unsigned int  NavPackeCount;
	unsigned char NavState;	
    int Longitude;
    int Latitude;
    int Altitude; 
    int rollAngleVelocity;
    int pitchAngleVelocity;
    int yawAngleVelocity;
    int Roll;
    int Pitch;
    unsigned int Yaw;
    short EaseVelocity;
    short NorthVelocity;
    short UpVelocity;
    short ResultVelocity;    
    int axAcceleration;  
    int ayAcceleration;  
    int azAcceleration;                    
    unsigned char NavFlag;	    
	unsigned short Checksum;    
} ImuDataStruct; //惯导到端机
// type 01
// ImuDataStruct;

typedef struct {
	unsigned char Head1;
	unsigned char Head2;    
	unsigned char DataLength;
	unsigned char DataType;
	unsigned char NodeID;
    unsigned int  GpsTime;    
    unsigned int  NavPackeCount;
	unsigned char NavState;	
    int Longitude;
    int Latitude;
    int Altitude; 
    int rollAngleVelocity;
    int pitchAngleVelocity;
    int yawAngleVelocity;
    int Roll;
    int Pitch;
    // unsigned int Yaw;   // imudata
    int Yaw;               // ctrldata
    short EaseVelocity;
    short NorthVelocity;
    short UpVelocity;
    short ResultVelocity;    
    int axAcceleration;  
    int ayAcceleration;  
    int azAcceleration;                    
    unsigned char NavFlag;	
    // unsigned short PodLaserDistance;   // 11-28
    // short PodpitchAngVelocity;
    // short PodyawAngVelocity;      
    unsigned char PodStatus1;
	unsigned char PodStatus2;
    unsigned char PodStatus3;
	unsigned char PodStatus4;
    short PodxShift;
    short PodyShift; 
    short PodRoll;
    short PodPitch;
    short PodYaw; 	
    short Podatt_Roll;
    short Podatt_Pitch;    
    short PodrollAngleVelocity;
    short PodpitchAngleVelocity;
    short PodyawAngleVelocity;  
    unsigned short PodLasDistance;   
    unsigned char PodChekFlag;  
	unsigned short Checksum;    
} CtrlDataStruct; //飞控到端机
// type 07
// CtrlStruct;

typedef struct {
	unsigned char Head1;
	unsigned char Head2;    
	unsigned char DataLength;
	unsigned char DataType;
	unsigned char NodeID;
    unsigned int  NavPackeCount;
	unsigned short LaserDistance;
    int Pitch;
    int Yaw; 	
    unsigned char SeekFlag;	    
	unsigned short Checksum;  
} SeekDataStruct;
// type 02

typedef struct
{
    unsigned char track_id;
    unsigned char width;
    unsigned char height;
    signed short x;
    signed short y;   
} DetTargetInfoStruct;

typedef struct
{
    DetTargetInfoStruct det_targetinfo;
    signed int longitude;
    signed int latitude;  
    signed int altitude; 
} LocTargetInfoStruct;

typedef struct
{
    DetTargetInfoStruct det_targetinfo;
    double longitude;
    double latitude;  
    double altitude;      
} Double_LocTargetInfoStruct;

typedef struct
{
    signed int longitude;
    signed int latitude;  
    signed int altitude; 
    unsigned char srcnodeid;
} FusionedTargetInfoStruct;

typedef struct
{
    unsigned char Head1;
	unsigned char Head2;    
	unsigned char DataLength;
	unsigned char DataType;
	unsigned char SrcNodeID;
    unsigned int  NavPackeCount;
    unsigned char NavState;
    int Longitude;
    int Latitude;
    int Altitude; 
    int Roll;
    int Pitch;
    // unsigned int Yaw;  //imudata
     int Yaw;    //ctrldata
    short EaseVelocity;
    short NorthVelocity;
    short UpVelocity;
    short ResultVelocity;    
    unsigned char MissionFlag;	    
    unsigned char TargetCount;
    LocTargetInfoStruct LocTargetInfo[5];
    signed int TrackLongitude;
    signed int TrackLatitude;  
    signed int TrackAltitude; 
    unsigned char StrikeCallFlag;
    unsigned char CallRespondFlag; 
    unsigned char RePlanFlag;
    unsigned int PlanLeftAllFlyTime;   
    unsigned int ArriveNextPointLeftFlyTime; 
    unsigned char FusionedTargetCount;  
    FusionedTargetInfoStruct FusionedTargetInfo[5];    
    unsigned char ZNDJChekFlag; 
    unsigned char IMUChekFlag; 
    unsigned char SEEKChekFlag; 
    unsigned char CTRLChekFlag;              
    unsigned char ScoutRegionSuccessFlag;
	unsigned short Checksum;
} ToDLDataStruct; //端机走数据链广播
// type 03
//DLDataStruct == ToDLDataStruct;

typedef struct {
	unsigned char Head1;
	unsigned char Head2;    
	unsigned char DataLength;
	unsigned char DataType;
	unsigned char NodeID;
    unsigned char NodesNum;
    unsigned int  TimeStamp;	    
	unsigned short Checksum;  
} DLTimeStruct;
// type 04

typedef struct
{
    signed int longitude;
    signed int latitude;  
    signed int altitude; 
} RegionInfoStruct;

typedef struct
{
    unsigned char Head1;
	unsigned char Head2;    
	unsigned char DataLength;
	unsigned char DataType;
	unsigned char SrcNodeID;
    RegionInfoStruct ScoutRegionInfo[8];
    RegionInfoStruct ForbidRegionInfo[8];             
	unsigned short Checksum;
} DLPreLoadStruct;
// type 05

typedef struct
{
    unsigned char RePlanFlag;
    signed int PlanPointLongitude;
    signed int PlanPointLatitude;
    signed int PlanPointAltitude;
    unsigned int PlanLeftAllFlyTime;
    unsigned int ArriveNextPointLeftFlyTime;
    signed short ArriveNextPointVelocity;
} AssignRoutePlanStruct;

typedef struct
{
    unsigned char Head1;
	unsigned char Head2;    
	unsigned char NodeID;
	unsigned char DataType;
    unsigned short DataLength;
    RegionInfoStruct ScoutRegionInfo[100];
    unsigned short Checksum;
} AssignRoutePlanToGndStruct;
// type 0E

typedef struct
{
	unsigned char Head1;
	unsigned char Head2;    
	unsigned char DataLength;
	unsigned char DataType;
	unsigned char NodeID;
    unsigned char CtrlInfo;
    unsigned char Reserve;       
    unsigned short Checksum;    
} GroundCtrlStruct;
// 55 AA 03 0F 00 00 00 12 00 (checksum 为３--n-1字节和)
// CtrlInfo: 01 取消跟踪，02 重新规划，03 退出任务

typedef struct {
	unsigned char Head1;
	unsigned char Head2;    
	unsigned char DataLength;
	unsigned char DataType;
	unsigned char NodeID;
    unsigned int  NavPackeCount;
	unsigned char NavState;	
    int Longitude;
    int Latitude;
    int Altitude; 
    int rollAngleVelocity;
    int pitchAngleVelocity;
    int yawAngleVelocity;    
    int Roll;
    int Pitch;
    unsigned int Yaw;
    short EaseVelocity;
    short NorthVelocity;
    short UpVelocity;
    short ResultVelocity;   
    int axAcceleration;  
    int ayAcceleration;  
    int azAcceleration;      
    unsigned char MissionFlag;	
    signed int TrackLongitude;
    signed int TrackLatitude;  
    signed int TrackAltitude;
    unsigned char StrikeCallFlag;
    unsigned char CallRespondFlag; 
    AssignRoutePlanStruct AssignRoutePlan;
    signed short StrikeTarget_x;
    signed short StrikeTarget_y; 
    unsigned char PodCtrinfo; 
    signed short PodVarX;
    signed short PodVarY;    
    unsigned char PodVarZ;
    char PodZoomVelocity;  
	unsigned short Checksum; 
} ToCtrlStruct;//端机到飞控
// type 06

typedef struct
{
    unsigned char NodeID;
    double Longitude;
    double Latitude;
    double Altitude; 
    double Roll;
    double Pitch;
    double Yaw;
    Double_LocTargetInfoStruct LocTargetInfo[5];
    unsigned char Missionflag;
    unsigned char Missioncluster;    
    double ActionTime;   
    double BufferTime;
} AssignDataStruct;

typedef struct
{
    unsigned char NodeID;
    double Longitude;
    double Latitude;
    double Altitude; 
    double Roll;
    double Pitch;
    double Yaw;
    unsigned char Missionflag;
    unsigned char Missioncluster;
} RouteDataStruct;

// QRD Pod struct
typedef struct {
	unsigned char Head1;
	unsigned char Head2;    
	unsigned char Status1;
	unsigned char Status2;
    unsigned short Status3;
    short xShift;
    short yShift; 
    short Roll;
    short Pitch;
    short Yaw; 	
    unsigned short Reserve1;	    
	unsigned short Reserve2;  
    short rollAngleVelocity;
    short pitchAngleVelocity;
    short yawAngleVelocity;  
    unsigned short LaserDistance;   
    unsigned char PodChekFlag;              
	unsigned short Reserve3;    
	unsigned char Checksum;            
} QRDPoddata;//吊舱到端机

typedef struct {
	unsigned char Head1;
	unsigned char Head2;    
	unsigned char CtrlInfo;
    short xVar;
    short yVar; 
    unsigned char zVar;
    char ZoomVelocity;              
	unsigned short Reserve1; 
	unsigned short Reserve2; 
	unsigned short Reserve3;            
	unsigned char Checksum;            
} QRDPodCtrldata;//端机到吊舱

#pragma pack(pop)

// IMU + UWB test CheckSum
uint8_t verifyCheckSum(const uint8_t *data, int32_t length)
{
    uint8_t sum = 0;
    for (int32_t i = 0; i < length - 1; ++i)
    {
        sum += data[i];
    }
    return sum == data[length - 1];
}

uint8_t verifyCheckSum_1(const uint8_t *data, int32_t length)
{
    unsigned short sum = 0;
    uint8_t checksum = 0;
    for (int32_t i = 0; i < length - 1; ++i)
    {
        sum += data[i];
    }
    checksum =(uint8_t)(sum % 256);
    return checksum == data[length - 1];
}

/***********************************************************************************
 * 函数名：unsigned char Datalink_GetChecksum(const unsigned char *DataPtr, unsigned short Num)
 * 功能：计算校验和
 * 参数：数据，数据长度
 * 返回值：校验和
 * 说明：无
 ***********************************************************************************/
unsigned char Datalink_GetChecksum(const unsigned char *DataPtr, unsigned short Num)
{
	unsigned char Checksum = 0;
	unsigned short Counter = 0;

	while(Counter < Num)
	{
		Checksum += *(DataPtr + Counter);
		Counter++;
	}

	return(Checksum);
}

/***********************************************************************************
 * 函数名：unsigned char Datalink_ChecksumIsOk(const unsigned char *DataPtr, unsigned short Num)
 * 功能：检测校验和是否正确
 * 参数：数据，数据长度
 * 返回值：TRUE正确，FALSE错误
 * 说明：无
 ***********************************************************************************/
unsigned char Datalink_ChecksumIsOk(const unsigned char *DataPtr, unsigned short Num)
{
	if( *(DataPtr + Num - 1) == Datalink_GetChecksum(DataPtr, Num-1) )
	{
		return true;
	}
	else
	{
		return false;
	}
}

/***********************************************************************************
 * 函数名：unsigned short DI_GetChecksum(const unsigned char *DataPtr, unsigned short Num)
 * 功能：计算校验和
 * 参数：数据，数据长度
 * 返回值：校验和
 * 说明：无
 ***********************************************************************************/
unsigned short DI_GetChecksum(const unsigned char *DataPtr, unsigned short Num)
{
	unsigned short Checksum = 0;
	unsigned short Counter = 0;

	while(Counter < Num)
	{
		Checksum += *(DataPtr + Counter);
		Counter++;
	}

	return(Checksum);
}


/***********************************************************************************
 * 函数名：unsigned char DI_ChecksumIsOk(const unsigned char *DataPtr, unsigned short Num)
 * 功能：检测校验和是否正确
 * 参数：数据，数据长度
 * 返回值：TRUE正确，FALSE错误
 * 说明：无
 ***********************************************************************************/
unsigned char DI_ChecksumIsOk(const unsigned char *DataPtr, unsigned short Num)
{
	if( *(unsigned short *)(DataPtr + Num - 2) == DI_GetChecksum(DataPtr, Num - 2) )
	{
		return true;
	}
	else
	{
		return false;
	}
}